#include "servo.h"
#include "math.h"

float Target1=810,Target2=813,Target3=530,Target4=779;     //���Ŀ��ֵ
float Velocity1,Velocity2,Velocity3,Velocity4;     //���PWM����
float Position1=810,Position2=813,Position3=530,Position4=779;    
float Speed=115;                   //�˶��ٶ�����
float	Position_KP_limb=20,Position_KI_limb=0,Position_KD_limb=10;  //λ�ÿ���PID����

float TargetX=1,TargetY=60,TargetZ=30,Target_claw=0;     					//���Ŀ��ֵ

u8 Servo_flag=0;

void Servo_move(void)
{
	u8 temp;

	
	temp=Kinematic_Analysis_limb(TargetX,TargetY,TargetZ,Target_claw);
	
	Xianfu_Pwm_limb();   //
	Velocity1=Position_PID1_limb(Position1,Target1);//���PID����
	Velocity2=Position_PID2_limb(Position2,Target2);//���PID����
	Velocity3=Position_PID3_limb(Position3,Target3);//���PID����
	Velocity4=Position_PID4_limb(Position4,Target4);//���PID����
	Xianfu_Pwm2_limb();
	Set_Pwm_limb(Velocity1,Velocity2,Velocity3,Velocity4);
	
}
void Servo_data(float x,float y,float z)
{
	TargetX=x;
	TargetY=y;
	TargetZ=z;
}

// void TIM1_PWM_Init(u16 arr,u16 psc) 
// {
	
// 	RCC->APB2ENR|=1<<11;       //ʹ��TIM1ʱ��   
// 	RCC->APB2ENR|=1<<6;        //PORTEʱ��ʹ�� 
// 	GPIOE->CRH&=0X000F0F0F;    //PORTE9 11 13 14 �������
// 	GPIOE->CRH|=0X8BB0B0B0;    //PORTE9 11 13 14 �������	  
	
// 	RCC->APB2ENR|=1<<0;     //��������ʱ��	   
// 	AFIO->MAPR&=0XFFFFFF3F; //���MAPR��[9:8]
// 	AFIO->MAPR|=3<<6;      //��ȫӳ��,TIM1_CH1->PE9 TIM1_CH2->PE11 TIM1_CH3->PE13 TIM1_CH4->PE14 
	
// 	TIM1->ARR=arr;             //�趨�������Զ���װֵ 
// 	TIM1->PSC=psc;             //Ԥ��Ƶ������Ƶ
// 	TIM1->CCMR1|=6<<4;         //CH1 PWM1ģʽ	
// 	TIM1->CCMR1|=6<<12;        //CH2 PWM1ģʽ	
// 	TIM1->CCMR2|=6<<4;         //CH3 PWM1ģʽ	
// 	TIM1->CCMR2|=6<<12;        //CH4 PWM1ģʽ	
	
// 	TIM1->CCMR1|=1<<3;         //CH1Ԥװ��ʹ��	  
// 	TIM1->CCMR1|=1<<11;        //CH2Ԥװ��ʹ��	 
// 	TIM1->CCMR2|=1<<3;         //CH3Ԥװ��ʹ��	  
// 	TIM1->CCMR2|=1<<11;        //CH4Ԥװ��ʹ��	  
	
// 	TIM1->CCER|=1<<0;         //CH1���ʹ��	
// 	TIM1->CCER|=1<<4;         //CH2���ʹ��	   
// 	TIM1->CCER|=1<<8;         //CH3���ʹ��	 
// 	TIM1->CCER|=1<<12;        //CH4���ʹ��
// 	TIM1->BDTR |= 1<<15;       //TIM����Ҫ��仰�������PWM
// 	TIM1->CR1=0x8000;          //ARPEʹ�� 
// 	TIM1->CR1|=0x01;          //ʹ�ܶ�ʱ�� 		
	
// }


/**************************************************************************
�������ܣ���ѧģ��
��ڲ�����ĩ��ִ����λ��̬
����  ֵ����
**************************************************************************/
u8 Kinematic_Analysis_limb(float x,float y,float z,float t) 
{
	float low,high,w,f,a,R_rot,R_side,rot,theta1,theta2,theta3,theta4;
		
//	R_rot=sqrt(x*x+y*y);
	R_rot=sqrt(y*y);
	R_side=sqrt(R_rot*R_rot+z*z);
	rot=x;
	high=2*acos(R_side/(2*150));
	w=asin(R_rot/R_side);
	a=(PI-high)/2;
	f=a-PI/2;
	if(z>=0)
		low=w+f;
	else low=PI-w+f;
	
	high=high+low;
	
//	theta1=rot*180/PI;
	theta1=rot;
	theta2=low*180/PI;
	theta3=high*180/PI-180;
	theta4=t;
	
	if(theta1>150)theta1=150;    
	if(theta1<-150)theta1=-150;  
	
	if(theta2>90)theta2=90;      //���ƶ�������Ƕȡ�90��
	if(theta2<-90)theta2=-90;    //���ƶ�������Ƕȡ�90��

	if(theta3>90)theta3=90;
	if(theta3<-90)theta3=-90;	    //���ƶ�������Ƕȡ�90��
	
	if(theta4>90)theta4=90;
	if(theta4<-90)theta4=-90;	    //���ƶ�������Ƕȡ�90��
	
	Target1   = 810-(theta1)*Ratio;
	Target2   = 810-(theta2)*Ratio1;
	Target3   = 810+(theta3)*Ratio1; 
	Target4   = 700-(theta4)*5.56; 
	
	
	return 0;
}


/**************************************************************************
�������ܣ���ֵ��PWM�Ĵ���,�����ж�ת��
��ڲ�������̨PWM������PWM���Ҷ��PWM
����  ֵ����
**************************************************************************/
void Set_Pwm_limb(float velocity1,float velocity2,float velocity3,float velocity4)
{	
		Position1+=velocity1;		   //�ٶȵĻ��֣��õ������λ��
		Position2+=velocity2;
		Position3+=velocity3;
		Position4+=velocity4;
	
	 PWM1=(int)Position1*2;    //��ֵ��STM32�ļĴ���
	 PWM2=(int)Position2*2;
	 PWM3=(int)Position3*2;
	 //PWM4=(int)Position4*2;
	//printf("%f,%f,%f,%f\n",Position1,Position2,Position3,Position4);
}
void Xianfu_Pwm_limb(void)
{	
	  int Amplitude_H=1250, Amplitude_L=250;       //�����������ֵ
    if(Target1<Amplitude_L)  Target1=Amplitude_L;	
		if(Target1>Amplitude_H)  Target1=Amplitude_H;	
	  if(Target2<Amplitude_L)  Target2=Amplitude_L;	
		if(Target2>Amplitude_H)  Target2=Amplitude_H;	
		if(Target3<Amplitude_L)  Target3=Amplitude_L;	
		if(Target3>Amplitude_H)  Target3=Amplitude_H;	
		if(Target4<Amplitude_L)  Target4=Amplitude_L;	
		if(Target4>Amplitude_H)  Target4=Amplitude_H;		
}
void Xianfu_Pwm2_limb(void)
{	
	  int Amplitude_H=Speed, Amplitude_L=-Speed;  
    if(Velocity1<Amplitude_L)  Velocity1=Amplitude_L;	
		if(Velocity1>Amplitude_H)  Velocity1=Amplitude_H;	
	  if(Velocity2<Amplitude_L)  Velocity2=Amplitude_L;	
		if(Velocity2>Amplitude_H)  Velocity2=Amplitude_H;	
		if(Velocity3<Amplitude_L)  Velocity3=Amplitude_L;	
		if(Velocity3>Amplitude_H)  Velocity3=Amplitude_H;	
		if(Velocity4<Amplitude_L)  Velocity4=Amplitude_L;	
		if(Velocity4>Amplitude_H)  Velocity4=Amplitude_H;	
}

/**************************************************************************
�������ܣ�λ��ʽPID������
��ڲ���������������λ����Ϣ��Ŀ��λ��
����  ֵ�����PWM
����λ��ʽ��ɢPID��ʽ 
pwm=Kp*e(k)+Ki*��e(k)+Kd[e��k��-e(k-1)]
e(k)��������ƫ�� 
e(k-1)������һ�ε�ƫ��  
��e(k)����e(k)�Լ�֮ǰ��ƫ����ۻ���;����kΪ1,2,,k;
pwm�������
**************************************************************************/
float Position_PID1_limb (float Encoder_limb,float Target_limb)
{ 	
	 static float Bias_limb,Pwm_limb,Integral_bias_limb,Last_Bias_limb;
	 Bias_limb=Target_limb-Encoder_limb;                                  //����ƫ��
	 Integral_bias_limb+=Bias_limb;	                                 //���ƫ��Ļ���
	 Pwm_limb=(Position_KP_limb-0)*Bias_limb/100+Position_KI_limb*Integral_bias_limb/100+(Position_KD_limb)*(Bias_limb-Last_Bias_limb)/100;       //λ��ʽPID������
	 Last_Bias_limb=Bias_limb;                                       //������һ��ƫ�� 
	 return Pwm_limb;                                           //�������
}
float Position_PID2_limb (float Encoder_limb,float Target_limb)
{ 	
	 static float Bias_limb,Pwm_limb,Integral_bias_limb,Last_Bias_limb;
	 Bias_limb=Target_limb-Encoder_limb;                                  //����ƫ��
	 Integral_bias_limb+=Bias_limb;	                                 //���ƫ��Ļ���
	 Pwm_limb=Position_KP_limb*Bias_limb/100+Position_KI_limb*Integral_bias_limb/100+Position_KD_limb*(Bias_limb-Last_Bias_limb)/100;       //λ��ʽPID������
	 Last_Bias_limb=Bias_limb;                                       //������һ��ƫ�� 
	 return Pwm_limb;                                           //�������
}float Position_PID3_limb (float Encoder_limb,float Target_limb)
{ 	
	 static float Bias_limb,Pwm_limb,Integral_bias_limb,Last_Bias_limb;
	 Bias_limb=Target_limb-Encoder_limb;                                  //����ƫ��
	 Integral_bias_limb+=Bias_limb;	                                 //���ƫ��Ļ���
	 Pwm_limb=Position_KP_limb*Bias_limb/100+Position_KI_limb*Integral_bias_limb/100+Position_KD_limb*(Bias_limb-Last_Bias_limb)/100;       //λ��ʽPID������
	 Last_Bias_limb=Bias_limb;                                       //������һ��ƫ�� 
	 return Pwm_limb;                                           //�������
}float Position_PID4_limb (float Encoder_limb,float Target_limb)
{ 	
	 static float Bias_limb,Pwm_limb,Integral_bias_limb,Last_Bias_limb;
	 Bias_limb=Target_limb-Encoder_limb;                                  //����ƫ��
	 Integral_bias_limb+=Bias_limb;	                                 //���ƫ��Ļ���
	 Pwm_limb=Position_KP_limb*Bias_limb/100+Position_KI_limb*Integral_bias_limb/100+Position_KD_limb*(Bias_limb-Last_Bias_limb)/100;       //λ��ʽPID������
	 Last_Bias_limb=Bias_limb;                                       //������һ��ƫ�� 
	 return Pwm_limb;                                           //�������
}

int myabs(int a)
{ 		   
	  int temp;
		if(a<0)  temp=-a;  
	  else temp=a;
	  return temp;
}

void hold()
{
	TIM4->CCR4=875*2;

}
void release()
{
	TIM4->CCR4=1160*2;
	
}
void center(void)
{
	Servo_data(7,50,40);
	HAL_Delay(700);
}
void pick_car_1(void)//从车上第一槽位拿起
{
	release();
	HAL_Delay(200);
	Servo_data(-45,25,20);
	HAL_Delay(800);
	Servo_data(-45,18,5);
	HAL_Delay(500);
	hold();
	HAL_Delay(500);
	Servo_data(-45,50,40);
	HAL_Delay(400);
	Servo_data(7,50,25);
}

void pick_car_2()//第二槽位拿起
{
	release();
	HAL_Delay(100);
	Servo_data(60,20,10);
	HAL_Delay(700);
	Servo_data(60,28,5);//ȡ��ɫ
	HAL_Delay(500);
	hold();
	HAL_Delay(400);
	Servo_data(60,25,30);
	HAL_Delay(600);
	Servo_data(7,50,40);
	HAL_Delay(200);
}
void pick_car_3()//第三槽位拿起
{
	release();
	HAL_Delay(200);
  	Servo_data(130,40,40);
	HAL_Delay(1000);
  	Servo_data(130,20,5);
	HAL_Delay(800);
	hold();
	HAL_Delay(500);
  	Servo_data(130,35,30);
	HAL_Delay(800);
	Servo_data(7,50,40);
	HAL_Delay(200);
}

void put_car_1()//放车第一槽位
{
	Servo_data(7,50,40);
	HAL_Delay(300);
	Servo_data(-45,35,30);
	HAL_Delay(1000);
	Servo_data(-45,35,15);
	HAL_Delay(500);
	release();
	HAL_Delay(400);
	Servo_data(7,50,40);
	HAL_Delay(200);
}
void put_car_2()//放车第二槽位
{
	Servo_data(7,50,40);
	HAL_Delay(800);
	Servo_data(65,33,15);
	HAL_Delay(1000);
	release();
	HAL_Delay(800);
	Servo_data(7,50,40);
	HAL_Delay(200);
}
void put_car_3()//放车第三槽位
{
	Servo_data(7,50,40);
	HAL_Delay(800);
  Servo_data(131,38,39);
	HAL_Delay(1000);
	Servo_data(131,36,14);
	HAL_Delay(800);
	release();
	HAL_Delay(300);
  Servo_data(130,35,30);
	HAL_Delay(800);
	Servo_data(7,50,40);
	HAL_Delay(200);
}
void put_ground_1()//放置在最左边
{
	Servo_data(-18,80,40);
	HAL_Delay(700);
	Servo_data(-23,121,-31);//������ɫ����ߣ�
	HAL_Delay(700);
	release();
	HAL_Delay(500);
	Servo_data(-20,40,0);//��ֹ��ȥ��ʱ��ײ��
	HAL_Delay(500);
	Servo_data(7,50,25);
}

void put_ground_2()
{
	Servo_data(10,85,30);
	HAL_Delay(800);
	Servo_data(10,98,-30);//������ɫ���м䣩
	HAL_Delay(700);
	release();
	HAL_Delay(800);
	Servo_data(10,50,-10);//��ֹ��ȥ��ʱ��ײ��
	HAL_Delay(500);
	Servo_data(7,50,25);
	HAL_Delay(500);
}
void put_ground_3()
{
	Servo_data(41.5,135,40);
	HAL_Delay(1000);
	Servo_data(41.5,140,-29);//���ں�ɫ���ұߣ�
	HAL_Delay(1000);
	release();
	HAL_Delay(500);
	Servo_data(45,80,0);//��ֹײ��
	HAL_Delay(500);
	Servo_data(7,50,40);
}

void peek_1()//从盘里里面抓
{
  Servo_data(7,50,40);
	HAL_Delay(1000);
	TIM4->CCR4=1160*2;
  HAL_Delay(1000);
	Servo_data(7,58,-10);
	HAL_Delay(1000);
  hold();
	HAL_Delay(1000);
	Servo_data(7,50,40);
	HAL_Delay(1000);
}
	
void pick_ground_1()//在最左边拿
{
	release();
	Servo_data(-18,80,40);
	HAL_Delay(700);
	Servo_data(-23,118,-30);//������ɫ����ߣ�
	HAL_Delay(700);
  hold();
	HAL_Delay(500);
	Servo_data(-20,40,0);//��ֹ��ȥ��ʱ��ײ��
	HAL_Delay(500);
	Servo_data(7,50,25);
}
void pick_ground_2()
{
	release();
	Servo_data(10,85,30);
	HAL_Delay(800);
	Servo_data(10,98,-35);//������ɫ���м䣩
	HAL_Delay(700);
  hold();
	HAL_Delay(800);
	Servo_data(10,50,-10);//��ֹ��ȥ��ʱ��ײ��
	HAL_Delay(500);
	Servo_data(7,50,25);
	HAL_Delay(500);
}

void pick_ground_3()
{
	release();	
	Servo_data(41.5,135,40);
	HAL_Delay(1000);
	Servo_data(41.5,140,-29);//���ں�ɫ���ұߣ�
	HAL_Delay(1000);
  hold();
	HAL_Delay(500);
	Servo_data(45,80,0);//��ֹײ��
	HAL_Delay(500);
	Servo_data(7,50,40);
}